#include <iostream>
#include <Utils.h>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <ostream>
using namespace std;
namespace dbg=lyxutils::debug;
namespace xio=lyxutils::io;
namespace str=lyxutils::str_utils;
namespace np=lyxutils::numpy;

void printHelp(xio::CLParser &parser){
    vector<string> optNames={"help","verbose","output","GSD","field","use-max-z","header","empty-cell","neighbors","basic-info-only"};
    vector<string> optDescr={"print this help message",
                             "execute in verbose mode(print out state information while program running)",
                             "set the output file name as FILE (text file) and write image top left corner coordinates and GSD to this file. if this option is not given, no output file will be generated. if this option is given but no argument offered, a default output.txt file will be generated in the current directory",
                             "set the image GSD(ground sample distance). if this option is not offered, the mean distance of point cloud will be used",
                             "which field to use as image pixel value. n is an ordinal number of the field (normally n=0,1,2...<==corresponds to==>x,y,z,r,g,b,other fields...),the default value is 2(z coordinate)",
                             "if multiple point fall in the same grid, use the field value of the point with maximum z coord;if this option is not set the average field value will be used instead",
                             "if this option is given , first line of the file will be considered as header.",
                             "set the method to deal empty cell as METHOD. options are:MIN,MAX,AVE,INT or a float value, if set MIN, emtpy cell will use minimum vale from its neighbors; if MAX->max value, AVE->average value; if set INT->interpolate from its neighbor values; if a float value,set as the user specified value. the default is AVE",
                             "number of neighbors used when deal with empty cell. the default value of NUM is 6",
                             "only get the basic statistics of the input point cloud,such as xmin,ymin,xmax,ymax,gsd being used,total point size,number of fields and field header(if header option offered)"};
    vector<string> optArgName={"","","FILE","gsd","n","","","METHOD","NUM",""};
    parser.addUsageExample("projection -vhbo pc.txt");
    parser.addUsageExample("projection -voz pc.txt img.jpg");
    parser.addUsageExample("projection -oz -e=INT pc.txt img.jpg cloud.txt");
    cout<<parser.generateHelp("projection [options...] <input-cloud> [output-image] [raster-cloud]","generate ortho-image of a point cloud.",optNames,optDescr,optArgName)<<endl;
}

int main(int argc,char **argv) {
    try{
        xio::CLParser parser;
        xio::CLOption options[]={
                {"help",0,false},
                {"verbose",0,true,'v'},
                {"output",1,true,'o'},
                {"GSD",2,true,'g'},
                {"field",2,true,'f'},
                {"use-max-z",0,true,'z'},
                {"header",0,true,'h'},
                {"empty-cell",2,true,'e'},
                {"neighbors",2,true,'n'},
                {"basic-info-only",0,true,'b'}
        };
        parser.setOptionPattern(10,options);
        parser.parse(argc,argv);
        //all parameters here
        bool help,vbs,use_max_z,hasHeader,basicInfo;
        int field,emptyMode,neighbors;
        float emptyVal;
        double gsd;
        string output,inFile,outImg,outCloud;
        if(argc>1&&!parser.hasOption("help")){
            //get parameters here
            [&](){
                output="";
                gsd=0;
                field=2;
                emptyMode=2;
                emptyVal=0;
                neighbors=6;
                vbs=parser.hasOption("verbose");
                if(parser.hasOption("output")){
                    if(parser.getOptionArg("output")!=""){
                        output=parser.getOptionArg("output");
                    }
                    else{
                        output="./output.txt";
                    }
                }
                if(parser.hasOption("GSD"))try{
                        gsd=stod(parser.getOptionArg("GSD"));
                        if(gsd<=0){
                            throw invalid_argument("gsd should be positive!");
                        }
                    }catch(exception e){
                        throw invalid_argument(e.what());
                    }
                if(parser.hasOption("field"))try{
                        field=stoi(parser.getOptionArg("field"));
                        if(field<2){
                            throw invalid_argument("field<2,use x or y coordinates as pixel value is meaningless!");
                        }
                    }catch(exception e){
                        throw invalid_argument(e.what());
                    }
                use_max_z=parser.hasOption("use-max-z");
                hasHeader=parser.hasOption("header");
                basicInfo=parser.hasOption("basic-info-only");
                if(parser.hasOption("empty-cell")){
                    if(parser.getOptionArg("empty-cell")=="MIN"){
                        emptyMode=0;
                    }
                    else if(parser.getOptionArg("empty-cell")=="MAX"){
                        emptyMode=1;
                    }
                    else if(parser.getOptionArg("empty-cell")=="AVE"){
                        emptyMode=2;
                    }
                    else if(parser.getOptionArg("empty-cell")=="INT"){
                        emptyMode=3;
                    }
                    else{
                        try{
                            emptyVal=stof(parser.getOptionArg("empty-cell"));
                            emptyMode=4;
                        }catch(exception e){
                            throw invalid_argument(e.what());
                        }
                    }
                }
                if(parser.hasOption("neighbors")){
                    try{
                        neighbors=stoi(parser.getOptionArg("neighbors"));
                        if(neighbors<1){
                            throw invalid_argument("number of neighbor(s) should be at least one!");
                        }
                    }catch(exception e){
                        throw invalid_argument(e.what());
                    }
                }
                vector<string> objs=parser.getParameters();
                if(objs.size()<1){
                    throw invalid_argument("this command requires at least one parameter, basic usage: projection [options...] <input-cloud> [output-image] [raster-cloud]");
                }
                inFile=objs[0];
                outImg="";
                outCloud="";
                if(objs.size()>1){
                    outImg=objs[1];
                }
                if(objs.size()>2){
                    outCloud=objs[2];
                }
                if(objs.size()<2&&!parser.hasOption("basic-info-only")){
                    throw invalid_argument("the name of the output image should be given unless --basic-info-only is provided-->see help");
                }
            }();
            //go on to process
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            vector<string> header;
            vector<vector<float> > fieldData;
            std::string report;
            //start process
            vbs && cout<<"[1/6]load point cloud file..."<<endl;
            bool read_state;
            string sep=" ,\t";
            if(hasHeader){
                if(vbs){
                    read_state=xio::read_pts_field(inFile,cloud->points,header,fieldData,sep,cout);
                }
                else{
                    read_state=xio::read_pts_field(inFile,cloud->points,header,fieldData,sep);
                }
            }
            else{
                if(vbs){
                    read_state=xio::read_pts_field(inFile,cloud->points,fieldData,sep,cout);
                }
                else{
                    read_state=xio::read_pts_field(inFile,cloud->points,fieldData,sep);
                }
            }
            if(!read_state)throw runtime_error("error when read point cloud file!");
            if(cloud->points.size()==0){
                cout<<"opened point cloud is empty!"<<endl;
            }
            float xmin,ymin,xmax,ymax,zmin;
            xmin=xmax=cloud->points[0].x;
            ymin=ymax=cloud->points[0].y;
            zmin=cloud->points[0].z;
            vbs && cout<<"[2/6]determine xmin,ymin,xmax,ymax..."<<endl;
            int portion=max((int)cloud->points.size()/100,1);
            for(int i=0;i<cloud->points.size();++i){
                xmin=min(xmin,cloud->points[i].x);
                xmax=max(xmax,cloud->points[i].x);
                ymin=min(ymin,cloud->points[i].y);
                ymax=max(ymax,cloud->points[i].y);
                zmin=min(zmin,cloud->points[i].z);
                if(vbs&&i%portion==0){
                    xio::consoleProgressBar(float(i)/cloud->points.size()*100);
                }
            }
            if(vbs)xio::consoleProgressBar(100);
            vbs && cout<<"xmin="<<xmin<<",xmax="<<xmax<<",ymin="<<ymin<<",ymax="<<ymax<<endl;
            vbs && cout<<"[3/6]determine gsd..."<<endl;
            if(gsd==0){
                vector<int> idx(neighbors+1);
                vector<float> dis(neighbors+1);
                double dis_sum;
                pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>);
                tree->setInputCloud(cloud);
                for(int i=0;i<cloud->points.size();++i){
                    float disAdd=0;
                    if(tree->nearestKSearch(cloud->points[i],neighbors+1,idx,dis)!=0){
                        for_each(dis.begin(),dis.end(),[&disAdd](float dis){disAdd+=sqrt(dis);});
                        disAdd/=neighbors;
                    }
                    gsd+=disAdd;
                    if(vbs&&i%portion==0){
                        xio::consoleProgressBar(float(i)/cloud->size()*100);
                    }
                }
                if(cloud->size()!=0)gsd/=cloud->size();
            }
            if(vbs)xio::consoleProgressBar(100);
            vbs && cout<<"gsd="<<gsd<<endl;
            int cols=ceil((xmax-xmin)/gsd);
            int rows=ceil((ymax-ymin)/gsd);
            [&](){
                //write basic information to output file
                if(output!=""){
                    ofstream ofs(output);
                    ofs<<"//basic info of point cloud file:"<<inFile<<endl;
                    ofs<<"number of points:"<<cloud->size()<<endl;
                    int dataCol=3;
                    if(fieldData.size()!=0)dataCol+=fieldData[0].size();
                    ofs<<"number of data columns:"<<dataCol<<endl;
                    if(hasHeader){
                        ofs<<"headers:";
                        for(int i=0;i<header.size();++i){
                            ofs<<header[i]<<",";
                        }
                        ofs<<endl;
                    }
                    ofs<<std::setprecision(8);
                    ofs<<"xmin="<<xmin<<",xmax="<<xmax<<",ymin="<<ymin<<",ymax="<<ymax<<",gsd="<<gsd<<endl;
                    ofs.flush();
                    if(ofs.is_open()){
                        ofs.close();
                    }
                }
            }();
            if(basicInfo){
                vbs && cout<<"only basic information is needed,the rest steps are cut off"<<endl;
                return 0;
            }
            vbs && cout<<"[4/6]projection/rasterization (project to XY plane)..."<<endl;
            vbs && cout<<"image rows="<<rows<<",cols="<<cols<<endl;
            cv::Mat img(rows,cols,CV_32FC1,cv::Scalar::all(zmin));//输出投影图像
            cv::Mat dem(rows,cols,CV_32FC1,cv::Scalar::all(zmin));//DEM图像
            cv::Mat mask(rows,cols,CV_32SC1,cv::Scalar::all(0));//每个像素的点计数
            cv::Mat mask8uc1(rows,cols,CV_8UC1,cv::Scalar::all(0));//哪些像素中有LiDAR点的掩膜
            for(int i=0;i<cloud->size();++i){
                int x=int((cloud->points[i].x-xmin)/gsd);
                int y=int((cloud->points[i].y-ymin)/gsd);
                mask.at<int>(y,x)++;
                mask8uc1.at<uchar>(y,x)=255;
                if(use_max_z){
                    if(dem.at<float>(y,x)<cloud->points[i].z){
                        img.at<float>(y,x)=dem.at<float>(y,x)=cloud->points[i].z;
                        if(field>=3){
                            img.at<float>(y,x)=fieldData[i][field-3];
                        }
                    }
                }
                else{
                    int cc=mask.at<int>(y,x);
                    dem.at<float>(y,x)=(dem.at<float>(y,x)*(cc-1)+cloud->points[i].z)/cc;
                    if(field>=3){
                        img.at<float>(y,x)=(img.at<float>(y,x)*(cc-1)+fieldData[i][field-3])/cc;
                    }
                    else{
                        img.at<float>(y,x)=dem.at<float>(y,x);
                    }
                }
                if(vbs&&i%portion==0){
                    xio::consoleProgressBar(float(i)/cloud->size()*100);
                }
            }
            vector<vector<cv::Point> > contours;
            cv::findContours(mask8uc1, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
            cv::Mat boundMask(rows, cols, CV_8UC1, cv::Scalar::all(0));
            cv::fillPoly(boundMask, contours, cv::Scalar::all(255));
            if(vbs)xio::consoleProgressBar(100);
            vbs && cout<<"[5/6]fill empty cell..."<<endl;
            vbs && cout<<"making 2d cloud for XY plane neareast points search..."<<endl;
            pcl::PointCloud<pcl::PointXY>::Ptr cloud2D(new pcl::PointCloud<pcl::PointXY>);
            vector<float> imgValidPixels;//投影图有LiDAR点的像素值,与cloud2D同size
            vector<float> demValidPixels;//DEM图像中有效像素点(有LiDAR点),与cloud2D同size
            float base_x=xmin+gsd/2;
            float base_y=ymin+gsd/2;
            cloud2D->height=cloud->height;
            cloud2D->width=cloud->width;
            cloud2D->is_dense=cloud->is_dense;
            cloud2D->points.reserve(cloud->size());
            int count=0;
            // for_each(cloud->points.begin(),cloud->points.end(),[&](const pcl::PointXYZ &p){
            //     cloud2D->points.push_back({p.x,p.y});
            //     if(vbs&&(++count%portion==0)){
            //         xio::consoleProgressBar(float(count)/cloud->points.size()*100);
            //     }
            // });
            for(int i=0;i<rows;++i){
                for(int j=0;j<cols;++j){
                    if(boundMask.at<uchar>(i,j)&&mask8uc1.at<char>(i,j)){
                        float x=base_x+gsd*j;
                        float y=base_y+gsd*i;
                        cloud2D->points.push_back({x,y});
                        demValidPixels.push_back(dem.at<float>(i,j));
                        imgValidPixels.push_back(img.at<float>(i,j));
                    }
                }
                if(vbs&&(++count%portion==0)){
                    xio::consoleProgressBar(float(count)/cloud->points.size()*100);
                }
            }
            pcl::KdTreeFLANN<pcl::PointXY>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXY>);
            tree->setInputCloud(cloud2D);
            if(vbs)xio::consoleProgressBar(100);
            vbs && cout<<"making raster cloud and filling empty cell..."<<endl;
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloudDEM(new pcl::PointCloud<pcl::PointXYZ>);
            int port2=max(img.rows/100,1);
            // float base_x=xmin+gsd/2;
            // float base_y=ymin+gsd/2;
            for(int i=0;i<rows;++i){
                int *pMask=mask.ptr<int>(i);
                float *pDem=dem.ptr<float>(i);
                float *pImg=img.ptr<float>(i);
                uchar *pBound=boundMask.ptr<uchar>(i);
                for(int j=0;j<cols;++j){
                    float x=base_x+gsd*j;
                    float y=base_y+gsd*i;
                    if(pBound[j]!=0){
                        if(pMask[j]==0){
                            vector<int> idx(neighbors);
                            vector<float> dis(neighbors);
                            tree->nearestKSearch({x,y},neighbors,idx,dis);
                            if(emptyMode==0){//use MIN
                                pDem[j]=demValidPixels[idx[0]];
                                pImg[j]=imgValidPixels[idx[0]];
                                // pDem[j]=cloud->points[idx[0]].z;
                                // if(field>=3){
                                //     pImg[j]=fieldData[idx[0]][field-3];
                                // }
                                // else{
                                //     pImg[j]=pDem[j];
                                // }
                            }
                            else if(emptyMode==1){//use MAX
                                pDem[j]=demValidPixels[idx[neighbors-1]];
                                pImg[j]=imgValidPixels[idx[neighbors-1]];
                                // pDem[j]=cloud->points[idx[neighbors-1]].z;
                                // if(field>=3){
                                //     pImg[j]=fieldData[idx[neighbors-1]][field-3];
                                // }
                                // else{
                                //     pImg[j]=pDem[j];
                                // }
                            }
                            else if(emptyMode==2){//use AVE
                                vector<float> zs;
                                // np::apply(idx,zs,[&](int i)->float{return cloud->points[i].z;});
                                np::apply(idx,zs,[&](int i)->float{return demValidPixels[i];});
                                pDem[j]=np::mean(zs);
                                np::apply(idx,zs,[&](int i)->float{return imgValidPixels[i];});
                                pImg[j]=np::mean(zs);
                                // if(field>=3){
                                //     vector<float> fs;
                                //     np::apply(idx,fs,[&](int i)->float{return fieldData[i][field-3];});
                                //     pImg[j]=np::mean(fs);
                                // }
                                // else{
                                //     pImg[j]=pDem[j];
                                // }
                            }
                            else if(emptyMode==3){//use INT
                                np::apply(dis,[&](float &f){f=sqrt(f);});
                                float dis_sum=np::sum(dis);
                                np::apply(dis,[&](float &f){f/=dis_sum;});
                                vector<float> zs;
                                // np::apply(idx,zs,[&](int i)->float{return cloud->points[i].z;});
                                np::apply(idx,zs,[&](int i)->float{return demValidPixels[i];});
                                pDem[j]=np::multiply(dis,zs);
                                np::apply(idx,zs,[&](int i)->float{return imgValidPixels[i];});
                                pImg[j]=np::multiply(dis,zs);
                                // if(field>=3){
                                //     vector<float> fs;
                                //     np::apply(idx,fs,[&](int i)->float{return fieldData[i][field-3];});
                                //     pImg[j]=np::multiply(dis,fs);
                                // }
                                // else{
                                //     pImg[j]=pDem[j];
                                // }
                            }
                            else{//use user defined value
                                vector<float> zs;
                                // np::apply(idx,zs,[&](int i)->float{return cloud->points[i].z;});
                                np::apply(idx,zs,[&](int i)->float{return demValidPixels[i];});
                                pDem[j]=np::mean(zs);
                                pImg[j]=emptyVal;
                            }
                        }
                        cloudDEM->push_back(pcl::PointXYZ(x,y,pDem[j]));
                    }
                }
                if(vbs&&i%port2==0){
                    xio::consoleProgressBar(float(i)/rows*100);
                }
            }
            if(vbs)xio::consoleProgressBar(100);
            vbs && cout<<"[6/6]output to files..."<<endl;
            cv::imwrite(outImg,img);
            cv::imwrite("mask.tiff",mask8uc1);
            if(outCloud!=""){//need to output raster point cloud
                xio::points3D_to_csv(outCloud,cloudDEM->points);
            }
        }
        else{
            printHelp(parser);
        }
    }catch(invalid_argument ia){
        cout<<"\ninvalid argument:"<<ia.what()<<endl;
    }
    catch(runtime_error re){
        cout<<"\nruntime error:"<<re.what()<<endl;
    }
    catch(exception e){
        cout<<"\nexception:"<<e.what()<<endl;
    }
    catch(...){
        cout<<"\nunknown error occurred!"<<endl;
    }
    return 0;
}
